/* EasyWay Game Engine
 * Copyright (C) 2006 Daniele Paggi.
 *  
 * Written by: 2006 Daniele Paggi<dshnt@hotmail.com>
 *   
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public License
 * as published by the Free Software Foundation; either version 2 of
 * the License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Library General Public License for more details.
 * 
 * You should have received a copy of the GNU Library General Public
 * License along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */
package org.easyway.collisions;

import org.easyway.geometry2D.Segment;
import org.lwjgl.util.vector.Matrix4f;
import org.lwjgl.util.vector.Vector3f;

public class TriangleCollision {

	private TriangleCollision() {
	}

	// private final static Vector3f vecX = new Vector3f(1, 0, 0);

	// private final static Vector3f vecY = new Vector3f(0, 1, 0);

	private final static Vector3f vecZ = new Vector3f(0, 0, 1);

	private static Segment sa, sb;

	// static boolean segmentTest;
	private static Vector3f V1, V2, V3;
	private static Vector3f U1, U2, U3;
	
	public static boolean test(Vector3f v1, Vector3f v2, Vector3f v3,
			Vector3f u1, Vector3f u2, Vector3f u3) {
		Vector3f Orig = new Vector3f(v1);

		Vector3f TEMP;

		V1 = new Vector3f(0, 0, 0);
		V2 = Vector3f.sub(v2, Orig, null);
		V3 = Vector3f.sub(v3, Orig, null);
		U1 = Vector3f.sub(u1, Orig, null);
		U2 = Vector3f.sub(u2, Orig, null);
		U3 = Vector3f.sub(u3, Orig, null);

		Vector3f norm = Vector3f.cross(V2, vecZ, null);
		float angle = Vector3f.angle(V2, vecZ);

		Matrix4f matrix = new Matrix4f();
		matrix.rotate(-angle, norm);
		rotateVectorByMatrix(V2, matrix);
		rotateVectorByMatrix(V3, matrix);
		rotateVectorByMatrix(U1, matrix);
		rotateVectorByMatrix(U2, matrix);
		rotateVectorByMatrix(U3, matrix);

		// segmentTest = false;
		approssimate();
		sa = new Segment(V1.x, V1.y, V2.x, V2.y);
		sb = new Segment(U1.x, U1.y, U2.x, U2.y);
		if (!sa.collision(sb)) {

			sb = new Segment(U1.x, U1.y, U3.x, U3.y);
			if (!sa.collision(sb)) {

				sb = new Segment(U2.x, U2.y, U3.x, U3.y);
				if (!sa.collision(sb)) {
					System.out.println("__1");
					return false;
				}
			}
		}
		// --------------- STEP 2 --------------------
		// Orig = ..;
		TEMP = V2;
		V2 = V3;
		V3 = TEMP;

		norm = Vector3f.cross(V2, vecZ, null);
		angle = Vector3f.angle(V2, vecZ);

		matrix = new Matrix4f();
		matrix.rotate(-angle, norm);
		rotateVectorByMatrix(V2, matrix);
		rotateVectorByMatrix(V3, matrix);
		rotateVectorByMatrix(U1, matrix);
		rotateVectorByMatrix(U2, matrix);
		rotateVectorByMatrix(U3, matrix);

		// segmentTest = false;
		approssimate();
		sa = new Segment(V1.x, V1.y, V2.x, V2.y);
		sb = new Segment(U1.x, U1.y, U2.x, U2.y);
		if (!sa.collision(sb)) {

			sb = new Segment(U1.x, U1.y, U3.x, U3.y);
			if (!sa.collision(sb)) {

				sb = new Segment(U2.x, U2.y, U3.x, U3.y);
				if (!sa.collision(sb)) {
					System.out.println("__2");
					return false;
				}
			}
		}

		// ---------------------- STEP 3 ---------------------
		Orig = new Vector3f(v2);
		V2 = Vector3f.sub(v3, Orig, null);
		V3 = Vector3f.sub(v1, Orig, null);
		U1 = Vector3f.sub(u1, Orig, null);
		U2 = Vector3f.sub(u2, Orig, null);
		U3 = Vector3f.sub(u3, Orig, null);

		norm = Vector3f.cross(V2, vecZ, null);
		angle = Vector3f.angle(V2, vecZ);

		matrix = new Matrix4f();
		matrix.rotate(-angle, norm);
		rotateVectorByMatrix(V2, matrix);
		rotateVectorByMatrix(V3, matrix);
		rotateVectorByMatrix(U1, matrix);
		rotateVectorByMatrix(U2, matrix);
		rotateVectorByMatrix(U3, matrix);

		// segmentTest = false;
		approssimate();
		sa = new Segment(V1.x, V1.y, V2.x, V2.y);
		sb = new Segment(U1.x, U1.y, U2.x, U2.y);
		if (!sa.collision(sb)) {

			sb = new Segment(U1.x, U1.y, U3.x, U3.y);
			if (!sa.collision(sb)) {

				sb = new Segment(U2.x, U2.y, U3.x, U3.y);
				if (!sa.collision(sb)) {
					System.out.println("__3");
					return false;
				}
			}
		}
		TEMP = v1;
		v1 = u1;
		u1 = TEMP;
		TEMP = v2;
		v2 = u2;
		u2 = TEMP;
		TEMP = v3;
		v3 = u3;
		u3 = TEMP;
		// --------- XXX -------------
		Orig = new Vector3f( v1 );
		V1 = new Vector3f(0, 0, 0);
		V2 = Vector3f.sub(v2, Orig, null);
		V3 = Vector3f.sub(v3, Orig, null);
		U1 = Vector3f.sub(u1, Orig, null);
		U2 = Vector3f.sub(u2, Orig, null);
		U3 = Vector3f.sub(u3, Orig, null);

		norm = Vector3f.cross(V2, vecZ, null);
		angle = Vector3f.angle(V2, vecZ);

		matrix = new Matrix4f();
		matrix.rotate(-angle, norm);
		rotateVectorByMatrix(V2, matrix);
		rotateVectorByMatrix(V3, matrix);
		rotateVectorByMatrix(U1, matrix);
		rotateVectorByMatrix(U2, matrix);
		rotateVectorByMatrix(U3, matrix);

		// segmentTest = false;
		approssimate();
		sa = new Segment(V1.x, V1.y, V2.x, V2.y);
		sb = new Segment(U1.x, U1.y, U2.x, U2.y);
		if (!sa.collision(sb)) {

			sb = new Segment(U1.x, U1.y, U3.x, U3.y);
			if (!sa.collision(sb)) {

				sb = new Segment(U2.x, U2.y, U3.x, U3.y);
				if (!sa.collision(sb)) {
					System.out.println("__4");
					return false;
				}
			}
		}
		// --------------- STEP 2 --------------------
		// Orig = ..;
		TEMP = V2;
		V2 = V3;
		V3 = TEMP;

		norm = Vector3f.cross(V2, vecZ, null);
		angle = Vector3f.angle(V2, vecZ);

		matrix = new Matrix4f();
		matrix.rotate(-angle, norm);
		rotateVectorByMatrix(V2, matrix);
		rotateVectorByMatrix(V3, matrix);
		rotateVectorByMatrix(U1, matrix);
		rotateVectorByMatrix(U2, matrix);
		rotateVectorByMatrix(U3, matrix);

		// segmentTest = false;
		approssimate();
		sa = new Segment(V1.x, V1.y, V2.x, V2.y);
		sb = new Segment(U1.x, U1.y, U2.x, U2.y);
		if (!sa.collision(sb)) {

			sb = new Segment(U1.x, U1.y, U3.x, U3.y);
			if (!sa.collision(sb)) {

				sb = new Segment(U2.x, U2.y, U3.x, U3.y);
				if (!sa.collision(sb)) {
					System.out.println("__5");
					return false;
				}
			}
		}

		// ---------------------- STEP 3 ---------------------
		V1 = new Vector3f(0,0,0);
		Orig = new Vector3f(v2);
		V2 = Vector3f.sub(v3, Orig, null);
		V3 = Vector3f.sub(v1, Orig, null);
		U1 = Vector3f.sub(u1, Orig, null);
		U2 = Vector3f.sub(u2, Orig, null);
		U3 = Vector3f.sub(u3, Orig, null);

		norm = Vector3f.cross(V2, vecZ, null);
		angle = Vector3f.angle(V2, vecZ);

		matrix = new Matrix4f();
		matrix.rotate(-angle, norm);
		rotateVectorByMatrix(V2, matrix);
		rotateVectorByMatrix(V3, matrix);
		rotateVectorByMatrix(U1, matrix);
		rotateVectorByMatrix(U2, matrix);
		rotateVectorByMatrix(U3, matrix);

		// segmentTest = false;
		approssimate();
		sa = new Segment(V1.x, V1.y, V2.x, V2.y);
		sb = new Segment(U1.x, U1.y, U2.x, U2.y);
		if (!sa.collision(sb)) {

			sb = new Segment(U1.x, U1.y, U3.x, U3.y);
			if (!sa.collision(sb)) {

				sb = new Segment(U2.x, U2.y, U3.x, U3.y);
				if (!sa.collision(sb)) {
					System.out.println("__6");
					return false;
				}
			}
		}
		return true;
	}
	
	private static final void approssimate() {
		V1.x = ((int)(1000*V1.x))*0.001f;
		V1.y = ((int)(1000*V1.y))*0.001f;
		V2.x = ((int)(1000*V2.x))*0.001f;
		V2.y = ((int)(1000*V2.y))*0.001f;
		U1.x = ((int)(1000*U1.x))*0.001f;
		U1.y = ((int)(1000*U1.y))*0.001f;
		U2.x = ((int)(1000*U2.x))*0.001f;
		U2.y = ((int)(1000*U2.y))*0.001f;
		U3.x = ((int)(1000*U3.x))*0.001f;
		U3.y = ((int)(1000*U3.y))*0.001f;
	}

	private static final void rotateVectorByMatrix(Vector3f v, Matrix4f m) {
		Vector3f t = new Vector3f();
		t.x = m.m00 * v.x + m.m01 * v.y + m.m02 * v.z;
		t.y = m.m10 * v.x + m.m11 * v.y + m.m12 * v.z;
		t.z = m.m20 * v.x + m.m21 * v.y + m.m22 * v.z;
		v.x = t.x;
		v.y = t.y;
		v.z = t.z;
	}

}
